//---------------------------------------------------------------------------

#include <vcl.h>
#pragma hdrstop

#include "LIPMCommunicator.h"
//---------------------------------------------------------------------------

#pragma package(smart_init)
#pragma link "YbCommDevice"
#pragma resource "*.dfm"
TFormLIPM *FormLIPM;

unsigned char Buf_LIPM[BUF_LIPM_SIZE] = {0};

//---------------------------------------------------------------------------

__fastcall TFormLIPM::TFormLIPM(TComponent* Owner)
	: TForm(Owner)
{
}
//---------------------------------------------------------------------------

void __fastcall TFormLIPM::Button_LIPM_CommSettingClick(TObject *Sender)
{
	YbCommDeviceLIPM->SettingsDialog(this,false);
}
//---------------------------------------------------------------------------

void __fastcall TFormLIPM::Button_LIPM_CommSwitchClick(TObject *Sender)
{
	try
	{
		YbCommDeviceLIPM->Active = !YbCommDeviceLIPM->Active;
	}
  catch(Exception &e)
	{
		ShowMessage("YbCommDeviceLIPM: "+e.Message);
	}

	if(YbCommDeviceLIPM->Active)
	{
		Button_LIPM_CommSwitch->Caption = "Close port";
		Timer_LIPM->Enabled = true;
	}
	else
	{
		Button_LIPM_CommSwitch->Caption = "Open port";
		Timer_LIPM ->Enabled = false;
  }
}
//---------------------------------------------------------------------------

void __fastcall TFormLIPM::Button_LIPM_GenerateMotionClick(TObject *Sender)
{
	unsigned char send_T[6] = {0};
	unsigned char send_L[6] = {0};
	unsigned char send_Generate[6] = {0};

	SetParameterConstraint();

	send_T[0] = SET_LIPM_PARAMETER_T;
	send_T[1] = (unsigned char)(StrToFloat(Edit_LIPM_T->Text)*100);
	send_T[2] = (unsigned char)(StrToFloat(Edit_LIPM_TimeSize->Text)*10);
	send_T[3] = (unsigned char)(StrToFloat(Edit_LIPM_dt->Text)*1000);
	send_T[4] = (unsigned char)(StrToFloat(Edit_LIPM_tSSP->Text)*100);

	send_L[0] = SET_LIPM_PARAMETER_L;
	send_L[1] = (unsigned char)(StrToFloat(Edit_LIPM_StepY->Text)*1000);
	send_L[2] = (unsigned char)(StrToFloat(Edit_LIPM_StepZ->Text)*1000);
	send_L[3] = (unsigned char)(StrToFloat(Edit_LIPM_MovingX->Text)*1000);
	if(StrToFloat(Edit_LIPM_RTheta->Text)<0)
	{
		send_L[4] = (unsigned char)(StrToFloat(Edit_LIPM_RTheta->Text)*(-10));
		send_L[4] = send_L[4] | 0x80;
	}
	else
	{
		send_L[4] = (unsigned char)(StrToFloat(Edit_LIPM_RTheta->Text)*10);
  }

	send_Generate[0] = GENERATE_LIPM_MOTION;
	send_Generate[1] = (unsigned char)(StrToFloat(Edit_LIPM_SpeedFactor->Text)*100);

	Send(send_T, 6);
	Send(send_L, 6);
	Send(send_Generate, 6);
}
//---------------------------------------------------------------------------

void TFormLIPM::SetParameterConstraint(void)
{
	if(StrToFloat(Edit_LIPM_T->Text) > 2.55)
	{
		Edit_LIPM_T->Text = "2.55";
	}
	else if(StrToFloat(Edit_LIPM_T->Text) < 0)
	{
		Edit_LIPM_T->Text = "0.65";
	}

	if(StrToFloat(Edit_LIPM_TimeSize->Text) > 25.5)
	{
		Edit_LIPM_TimeSize->Text = "25.5";
	}
	else if(StrToFloat(Edit_LIPM_TimeSize->Text) < 0)
	{
		Edit_LIPM_TimeSize->Text = "2.6";
	}

	if(StrToFloat(Edit_LIPM_dt->Text) > 0.255)
	{
		Edit_LIPM_dt->Text = "0.255";
	}
	else if(StrToFloat(Edit_LIPM_dt->Text) < 0.03)
	{
		Edit_LIPM_dt->Text = "0.03";
	}

	if(StrToFloat(Edit_LIPM_tSSP->Text) > 2.55)
	{
		Edit_LIPM_tSSP->Text = "2.55";
	}
	else if(StrToFloat(Edit_LIPM_tSSP->Text) < 0)
	{
		Edit_LIPM_tSSP->Text = "0.4";
	}

	if(StrToFloat(Edit_LIPM_StepY->Text) > 0.255)
	{
		Edit_LIPM_StepY->Text = "0.255";
	}
	else if(StrToFloat(Edit_LIPM_StepY->Text) < 0)
	{
		Edit_LIPM_StepY->Text = "0.055";
	}

	if(StrToFloat(Edit_LIPM_StepZ->Text) > 0.255)
	{
		Edit_LIPM_StepZ->Text = "0.255";
	}
	else if(StrToFloat(Edit_LIPM_StepZ->Text) < 0)
	{
		Edit_LIPM_StepZ->Text = "0.032";
	}

	if(StrToFloat(Edit_LIPM_MovingX->Text) > 0.255)
	{
		Edit_LIPM_MovingX->Text = "0.255";
	}
	else if(StrToFloat(Edit_LIPM_MovingX->Text) < 0)
	{
		Edit_LIPM_MovingX->Text = "0.01";
	}

	if(StrToFloat(Edit_LIPM_RTheta->Text) > 12.7)
	{
		Edit_LIPM_RTheta->Text = "12.7";
	}
	else if(StrToFloat(Edit_LIPM_RTheta->Text) < -12.7)
	{
		Edit_LIPM_RTheta->Text = "-12.7";
	}

	if(StrToInt(Edit_LIPM_RepeatCount->Text) > 15)
	{
		Edit_LIPM_RepeatCount->Text = "15";
	}
	else if(StrToInt(Edit_LIPM_RepeatCount->Text) < 1)
	{
		Edit_LIPM_RepeatCount->Text = "1";
	}

	if(StrToFloat(Edit_LIPM_SpeedFactor->Text) > 1.5)
	{
		Edit_LIPM_SpeedFactor->Text = "1.5";
	}
	else if(StrToFloat(Edit_LIPM_SpeedFactor->Text) < 0.1)
	{
		Edit_LIPM_SpeedFactor->Text = "0.1";
	}
}
//---------------------------------------------------------------------------

void __fastcall TFormLIPM::Button_LIPM_RunMotionClick(TObject *Sender)
{
	unsigned char send_Run[6] = {0};
	send_Run[0] = RUN_LIPM_MOTION;
	send_Run[1] = StrToInt(Edit_LIPM_RepeatCount->Text);

	SetParameterConstraint();
	Send(send_Run, 6);
}
//---------------------------------------------------------------------------

void __fastcall TFormLIPM::Button_LIPM_MemoClearClick(TObject *Sender)
{
	Memo_LIPM_Show->Clear();
}
//---------------------------------------------------------------------------

void __fastcall TFormLIPM::Timer_LIPMTimer(TObject *Sender)
{
	int Num_read_byte = YbCommDeviceLIPM->Read(Buf_LIPM, BUF_LIPM_SIZE);

	if(CheckBox_Numerical->Checked)
	{
		String s_read = ">> ";
		s_read = s_read + IntToStr(Buf_LIPM[0]);
		for(int i = 1; i < Num_read_byte; i++)
		{
			s_read = s_read + ", " + IntToStr(Buf_LIPM[i]);
		}
		Memo_LIPM_Show->Lines->Add(s_read);
	}
	else
	{
		int scanned_byte = 0;
		int index = 0;
		unsigned char temp_buf[BUF_LIPM_SIZE] = {0};
		while (scanned_byte < Num_read_byte)
		{
			if(Buf_LIPM[scanned_byte]!=10) // \n
			{
				temp_buf[index] = Buf_LIPM[scanned_byte];
				index++;
				scanned_byte++;
			}
			else
			{
				Memo_LIPM_Show->Lines->Add((AnsiString)(char *)temp_buf);
				for(int i = 0; i < index; i++)
				{
					temp_buf[i] = 0;
				}
				index = 0;
				scanned_byte++;
			}
		}
		Memo_LIPM_Show->Text = Memo_LIPM_Show->Text + (AnsiString)(char *)temp_buf;
  }
}
//---------------------------------------------------------------------------

void __fastcall TFormLIPM::Button_LIPM_ShowParameterClick(TObject *Sender)
{
	unsigned char send_Show[6] = {0};
	send_Show[0] = SHOW_LIPM_PARAMETER;

	Send(send_Show, 6);
}
//---------------------------------------------------------------------------

void TFormLIPM::Send(unsigned char *Tx, int length)
{
	String s_read = "<< ";
	unsigned char checksum = 0;
	for(int i=0; i<5; i++)
	{
		checksum += Tx[i];
		s_read += IntToStr(Tx[i]) + ", ";
	}
	Tx[5] = ~checksum;
	s_read += Tx[5];
	Memo_LIPM_Show->Lines->Add(s_read);

	if(YbCommDeviceLIPM->Active)
	{
		YbCommDeviceLIPM->OutQueue->Clear();
		YbCommDeviceLIPM->Write(Tx, 6);
	}
}
